#include "../_includes.h"
timer t_auto;
int timeout_toarm = 4000;
void clear_auto_info_line(int32_t x, int32_t y, fontType ft)
{
  screen_set_pen_font(ft);
  Print(x, y, "                                                        ");
}
void out_ring()
{
  suck.set_voltage(-100);
  chain.set_voltage(-100);
}
void get_ring()
{
  suck.set_voltage(100);
  chain.set_voltage(80);

  // 阻塞情况处理
  //   const double thresholdCurrent =5.0;
  //  // 设置电流阈值
  //  const double thresholdVelocity =10.0;
  //  // 设置速度阈值

  //  // 检查电机电流和速度
  //   double current = mtr_chain.current(percentUnits::pct);
  //   std::cout <<current << std::endl;
  //   double velocity = mtr_chain.velocity(percentUnits::pct);
  //   std::cout<< velocity << std::endl;
  //   int time_error=2000;
  //   timer solve_time;
  //   solve_time.clear();

  //  // 如果电机电流高于阈值且速度低于阈值，表示可能被卡住
  //  if (current > thresholdCurrent && velocity < thresholdVelocity) {
  //   while (solve_time.time()<time_error)
  //       {
  //         mtr_chain.spin(directionType::fwd, -100, velocityUnits::pct);
  //       }
  //       chain.stop();
  //  }
}

void stop_getring()
{
  suck.stop();
  chain.stop();
}

void hold_inside()
{
  while (1)
  {
    if (vis.takeSnapshot(1) && vis.largestObject.width > 200 && vis.largestObject.height > 100)
    {
      vis.setLedColor(255, 0, 0);
      cout << "REDREDRED" << endl;
      stop_getring();
      this_thread::sleep_for(100);
    }
    this_thread::sleep_for(5);
  }
}

void auto_toarm()
{
  int getnum = 0;
  timer toarmtime;
  toarmtime.clear();
  while (getnum < 2 && toarmtime.time() < timeout_toarm)
  {
    if ((vis.takeSnapshot(1) && vis.largestObject.width > 200 && vis.largestObject.height > 100) || (vis.takeSnapshot(2) && vis.largestObject.width > 200 && vis.largestObject.height > 100))
    {
      suck.stop();
      getnum++;
      vis.setLedColor(255, 0, 0);
      stop_getring();
      arm.act_for_enc_absolute(0, 100, 500, Block::no);
      int startcode = mtr_chain.position(rotationUnits::deg) / 1300;
      int aimcode = startcode * 1300 + 1100;
      while (mtr_chain.position(rotationUnits::deg) < aimcode)
      {
        mtr_chain.spin(directionType::fwd, 100, velocityUnits::pct);
      }
      chain.stop();
      this_thread::sleep_for(200);
      aimcode = (startcode - 1) * 1300;
      while (mtr_chain.position(rotationUnits::deg) > aimcode)
      {
        mtr_chain.spin(directionType::fwd, -100, velocityUnits::pct);
      }
      chain.stop();
      this_thread::sleep_for(100);
      get_ring();
    }
    // else
    // {
    //   get_ring();
    // }
  }
}
void auto_arm_holdinside_blue()
{
  while (1)
  {
    if ((vis.takeSnapshot(1) && vis.largestObject.width > 200 && vis.largestObject.height > 100))
    {
      suck.stop();
      vis.setLedColor(255, 0, 0);
      stop_getring();
      arm.act_for_enc_absolute(0, 100, 500, Block::no);
      int startcode = mtr_chain.position(rotationUnits::deg) / 1300;
      int aimcode = startcode * 1300 + 1100;
      while (mtr_chain.position(rotationUnits::deg) < aimcode)
      {
        mtr_chain.spin(directionType::fwd, 100, velocityUnits::pct);
      }
      chain.stop();
      this_thread::sleep_for(200);
      aimcode = (startcode - 1) * 1300;
      while (mtr_chain.position(rotationUnits::deg) > aimcode)
      {
        mtr_chain.spin(directionType::fwd, -100, velocityUnits::pct);
      }
      chain.stop();
      get_ring();
    }
    if (vis.takeSnapshot(2) && vis.largestObject.width > 200 && vis.largestObject.height > 100)
    {
      vis.setLedColor(255, 0, 0);
      stop_getring();
    }
    this_thread::sleep_for(5);
  }
}